#include "StateSignalProcessor.h"

ros::Publisher inter_Pub;

saint_nr_9::IRdata current_irdata;
std_msgs::Int32 current_motor_state;
std_msgs::Int32 current_tag_state;
std_msgs::Int32 current_tag_f_state;
std_msgs::Int32 current_proc_f_state;
std_msgs::Int32 current_path_state;
std_msgs::Int32 current_activ_state;
std_msgs::Int32 current_activ2_state;

int main(int argc, char**argv) {
  ros::init(argc, argv, "StateSignalProcessor");
  ros::NodeHandle n;
  inter_Pub = n.advertise<std_msgs::Int32>("Signals/state_signal", 1);
  ros::Subscriber sub_ir = n.subscribe("ir_info", 1, ir_callback);
  ros::Subscriber sub_motor = n.subscribe("Signals/state_motor", 1, motor_callback);
  ros::Subscriber sub_tag = n.subscribe("Signals/state_tag", 1, tag_callback);
  ros::Subscriber sub_tag_f = n.subscribe("Signals/state_tag_finish", 1, tag_f_callback);
  ros::Subscriber sub_proc_f = n.subscribe("Signals/state_process_finish", 1, proc_f_callback);
  ros::Subscriber sub_path = n.subscribe("Signals/state_path", 1, path_callback);
  ros::Subscriber sub_activ = n.subscribe("Signals/Activation", 1, activ_callback);
  ros::Subscriber sub_activ2 = n.subscribe("Signals/Activation2", 1, activ2_callback);
  current_path_state.data = -1;
  printf("StateSignalProcessor running\n");
  ros::spin();
}

void activ2_callback(const std_msgs::Int32::ConstPtr& data)
{
  //cout << "Activation" << endl;
  current_activ2_state = *data;
  send_signal();
  current_activ_state.data = 0;  
}

void activ_callback(const std_msgs::Int32::ConstPtr& data)
{
  //cout << "Activation" << endl;
  current_activ_state = *data;
  send_signal();
  current_activ_state.data = 0;  
}
void path_callback (const std_msgs::Int32::ConstPtr& data)
{
  cout << "State" << data->data << endl;
  current_path_state = *data;
  send_signal();
  current_path_state.data = -1;
  //printf("HUII\n");  
}

void proc_f_callback (const std_msgs::Int32::ConstPtr& data)
{
  current_proc_f_state = *data;
  send_signal();
  current_proc_f_state.data = 0;
}

void tag_f_callback (const std_msgs::Int32::ConstPtr& data)
{
  //printf("Tag Finish Signal \n");
  current_tag_f_state = *data;
  send_signal();
  current_tag_f_state.data = 0;
}

void tag_callback(const std_msgs::Int32::ConstPtr& data)
{
  //printf("Tag Signal \n");
  current_tag_state = *data;
  send_signal();
  current_tag_state.data = 0;
}

void ir_callback(const saint_nr_9::IRdata::ConstPtr& data)
{
  current_irdata = *data;
  send_signal();
}

void motor_callback(const std_msgs::Int32::ConstPtr& data)
{
  current_motor_state = *data;
  send_signal();
  current_motor_state.data = 0;
}

void send_signal()
{
  std_msgs::Int32 signal;
  signal.data = 0;
  if(current_irdata.blue)
  {
    signal.data += (1<<BUMPER);
  }
  
  if (current_irdata.white < RIGHT_THRESHOLD)
  {
    if (current_irdata.orange < RIGHT_THRESHOLD)
    { 
      signal.data += (1<<C_C_RIGHT);
    }
    else 
    {
      signal.data += (1<<C_F_RIGHT);
    }
  }
  else 
  {
    if (current_irdata.orange < RIGHT_THRESHOLD)
    {
      signal.data += (1<<F_C_RIGHT);
    }
    else if (current_irdata.orange >= RIGHT_THRESHOLD)
    {
      signal.data += (1<<F_F_RIGHT); 
    }
  }
  
  if(current_irdata.red < FRONT_THRESHOLD)
  {
    if (current_irdata.yellow < FRONT_THRESHOLD)
    {
      signal.data += (1<<C_C_FRONT);
    }
    else
    {
      signal.data += (1<<C_F_FRONT);
    }
    
  }
  else
  {
    if (current_irdata.yellow < FRONT_THRESHOLD)
    {
      signal.data += (1<<F_C_FRONT);
    }
    else
    {
      signal.data += (1<<F_F_FRONT);
    }
  }
  
  signal.data += (current_motor_state.data<<MOTOR_DONE);
  //for Christoffer ;) nooooo not only, its really nice :)
  signal.data += (current_tag_state.data<<TAG_DETECTED);
  signal.data += (current_tag_f_state.data<<TAG_DETECTION_FINISHED);
  signal.data += (current_proc_f_state.data<<TAG_PROCESSING_FINISHED);
  signal.data += (current_activ_state.data<<ACTIVATE1);
  signal.data += (current_activ2_state.data<<ACTIVATE2);
  
  if (current_path_state.data !=-1)
  {
   //printf("Hui Signal \n");
    if(current_path_state.data == PATH_STRAIGHT)
    {
      cout << "Yep" << endl;
      signal.data += (1<<FOLLOW_PATH_STRAIGHT);
    }
    else if(current_path_state.data == PATH_LEFT_TURN)
    {
      signal.data += (1<<FOLLOW_PATH_LEFT);
    }
    else if(current_path_state.data == PATH_RIGHT_TURN)
    {
      signal.data += (1<<FOLLOW_PATH_RIGHT);
    }
    else if(current_path_state.data == PATH_LEFT_TURN)
    {
      signal.data += (1<<FOLLOW_PATH_LEFT);
    }
    else if(current_path_state.data == PATH_U_TURN)
    {
      signal.data += (1<<FOLLOW_PATH_U_TURN);
    }
    else if(current_path_state.data == PATH_SEARCH_TAG)
    {
      signal.data += (1<<FOLLOW_PATH_SEARCH_TAG);
    }
    else
    {
      printf("Uhhhhhh \n ");
    }
  }
  
  inter_Pub.publish(signal);
  //ROS_INFO("Signal: %s\n", byte_to_binary(signal.data));
}

const char *byte_to_binary(int x)
{
    static char b[9];
    b[0] = '\0';

    int z;
    for (z = 128; z > 0; z >>= 1)
    {
        strcat(b, ((x & z) == z) ? "1" : "0");
    }

    return b;
}
